//==========================================================
//	TaskControl.cpp
//	控制任务执行的相关类的实现
//==========================================================
#include <stdio.h>
#include "global.h"
#include "util.h"
#include "ImxErr.h"


using namespace Imx;
using namespace std;

extern void print_frame(struct can_frame *frame);
//======Loop=======================================
Loop::Loop(s32 head, s32 need)
{
	_head = head;
	_need = need;
	_cnt = 0;
}

s32
Loop::need()
{
	return _need;
}

void
Loop::cnt()
{
	_cnt += 1;
}

void
Loop::cnt(int cnt)
{
	_cnt = cnt;
}

s32
Loop::getCnt()
{
	return _cnt;
}

s32
Loop::head()
{
	return _head;
}
//======Task Loader============================================
TaskSchedule::TaskSchedule()
{
	_stepIdx = 0;
	printf("........................................................sub create !\n");
}

TaskSchedule::~TaskSchedule()
{
}

void
TaskSchedule::run()
{
	// post protect
	u8 devId,chnNo;
	s32 ret = -1;
	u32	can_id;
	Step step;
	_stopped = false;// 默认为运行状态
	_flag = 0x1; // 运行状态
	devId = getID();
	chnNo = getChn();

	printf("\n>>>>>>>>>>>>1\n");
	// 记录任务开始时间
	clock_gettime(CLOCK_REALTIME,&_taskStartTs);
	// set task name
	printf(" %x  %d   %s \n",devId,chnNo,_task.name.c_str());
	gDataManager.chnSetName(devId, chnNo, _task.name);
	while(!_stopped)
	{
		// _flag check
		if(_flag == 0x1) // 开始 继续
		{
		}
		else if(_flag == 0x2) // 暂停
		{
			while(_flag == 0x2)
			{
				sleep(1);
			}
			if(_flag == 0x4) // 判断结束暂停状态时，若切换为停止状态则跳出循环
			{
				break;
			}
		}

	printf("\n>>>>>>>>>>>>1\n");
		step = popStep();
		// post single step 
		while(1)
		{
			// 发布工步
			can0.stepNotify(devId, chnNo, step);
			gDataManager.setChnEnd(devId, chnNo, step); // 记录截止条件
			printf("[%s] Step Notify [%x][%x]!\n",__func__,devId,chnNo);

			// 确认工步接收
			ret = waitAck(&can_id, 5); // 5秒超时
			printf("[%s] Wait Ack ret[%d]!\n",__func__,ret);
			if(ret == IE_ACK_TIMEOUT)
			{
				printf("[%s] ack time out\n",__func__);
			}
			if(ret == IE_OK)
			{
				printf("[%s] ie ok\n",__func__);
				break; 
			}

		}
		// 记录工步开始时间
		clock_gettime(CLOCK_REALTIME,&_stepStartTs);


		// 等待工步结束
		ret = waitAck(&can_id, 0);	// 0 表示无限等待
		// TODO wait for reaching step finish condition
		//ret = waitFinish(&can_id, 0); // 永久等待
		// debug step finish after 5 secs
		//sleep(5); // debug time interval
	}

	printf("================> finish notify !\n");
	while(1)
	{
		can0.finishNotify(devId,chnNo);
		ret = waitAck(&can_id, 5);
		if(ret == IE_ACK_TIMEOUT)
		{
			printf("[%s]finish ack time out\n",__func__);
		}
		if(ret == IE_OK)
		{
			printf("[%s]finish ie ok\n",__func__);
			break; 
		}
	}
	gTaskManager.clearTask(devId, chnNo);
	printf("...>>> task table clear , finish!\n");
	// finish work, notify
}

// 目前的算法不支持单个工步循环，循环节点不重可重叠
Step
TaskSchedule::popStep()
{
	Step ss;
	ss = _task.steps[_stepIdx];

	if(ss.loopStart)
	{
		_hasLoop = true;
		if(_loopStk.empty() || (_loopStk.back().head()!=ss.seq))
		{
			Loop loop(ss.seq,ss.loopCnt);
			_loopStk.push_back(loop);
		}
	}

	if(ss.loopEnd)
	{
		_hasLoop = true;
		_loopStk.back().cnt(); // 循环结尾循环执行次数 +1
		printf("current loop [%d] ~ [%d] needed[%d] excuted[%d]\n",_loopStk.back().head(),ss.seq,_loopStk.back().need(),_loopStk.back().getCnt());
		if(_loopStk.back().getCnt() < _loopStk.back().need()) // 循环执行次数小于要求次数
		{
			_stepIdx = _loopStk.back().head(); // 工步索引重新指向循环开始工步;
		}
		else // 循环执行次数达到要求次数
		{
			_loopStk.pop_back(); // 释放当前循环
			_stepIdx++;
			printf("exit loop\n");
		}
	}
	else
	{
		_stepIdx++;
	}


	if(_stepIdx < _task.steps.size())
	{
	}
	else // 任务最终工步
	{
		if(_hasLoop)
		{
			if(_loopStk.back().getCnt() < _loopStk.back().need()) // 循环执行次数小于要求次数
			{
				_stepIdx = _loopStk.back().head(); // 工步索引重新指向循环开始工步;
			}
			else // 循环执行次数达到要求次数
			{
				printf("[%s] set _load false.  _stepIdx[%d]\n",__func__,_stepIdx);
				_stopped = true; // 重置任务加载标识 当前线程循环在下一次退出
				//_stepIdx = 0; // 重置工步索引
			}
		}
		else
		{
			_stopped = true;
		}
	}
	return ss;
}

u8
TaskSchedule::getID()
{
	return _devId;
}

u8
TaskSchedule::getChn()
{
	return _chnNo;
}

// 获取任务执行时间
u32
TaskSchedule::getTaskElapsed()
{
	struct timespec ts;
	u32	ms;

	clock_gettime(CLOCK_REALTIME,&ts);
	ms = (ts.tv_sec - _taskStartTs.tv_sec)*1000 + (ts.tv_nsec-_taskStartTs.tv_nsec)/1000000;
	return ms;
}

// 获取工步执行时间
u32
TaskSchedule::getStepElapsed()
{
	struct timespec ts;
	u32	ms;

	clock_gettime(CLOCK_REALTIME,&ts);
	ms = (ts.tv_sec - _stepStartTs.tv_sec)*1000 + (ts.tv_nsec-_stepStartTs.tv_nsec)/1000000;
	return ms;
}

bool
TaskSchedule::isLoad()
{
	return _load;
}

void
TaskSchedule::pause()
{
	_flag = 0x2;
	printf(">>>...pause called!\n");
}

void
TaskSchedule::cont()
{
	_flag = 0x1;
	printf(">>>...cont called!\n");
}

// 停止当前线程
void
TaskSchedule::stop()
{
	_stopped = true;
	_flag = 0x4;
	printf(">>>...stop called!\n");
}

s32
TaskSchedule::waitAck(u32 *can_id, s32 timeout)
{
	IceUtil::Monitor<IceUtil::Mutex>::Lock lock(*this);
	struct can_frame frame;
	s32 ret;

	if(timeout > 0)
	{
		if(_ackQueue.empty())
		{
			IceUtil::Time time = IceUtil::Time::seconds(timeout);
			timedWait(time);
		}

		if(_ackQueue.empty())
			return IE_ACK_TIMEOUT;
	}
	if(timeout == 0)
	{
		while(_ackQueue.empty())
		{
			wait();
		}
	}
	frame = _ackQueue.front();
	_ackQueue.pop();
	*can_id = frame.can_id;
	print_frame(&frame);
	memcpy(&ret, &frame.data, 4); // 提取下位机返回值
	printf(">>>>>>>>>>>>>>%d\n",ret);
	return ret;
}

void
TaskSchedule::putAck(struct can_frame frame)
{
	IceUtil::Monitor<IceUtil::Mutex>::Lock lock(*this);
	_ackQueue.push(frame);
	if(_ackQueue.size() == 1)
	{
		notify();
	}
}
//======Task Manager============================================
TaskManager::TaskManager()
{
	//_taskThreads.resize(1);
	// init thread matrix
	int i,j;
	for(i=0;i<MAX_DEVS;i++)
	{
		for(j=0;j<MAX_CHN;j++)
		{
			_taskTab[i][j] = NULL;
		}
	}
}

// 加载任务
// 负责设置保护条件与任务的加载启动
s32
TaskManager::loadTask(Task task)
{
	u8	devId,chnNo,i;
	s32	ret;
	u32	can_id;
	TaskProtect tp; // 任务保护条件 32 = 30 data + 2 crc

	devId = task.devId;
	chnNo = task.chnNo;
	printf(">>>>task name [%s]  seq[%d]  devId[%x] chnNo[%d]\n", task.name.c_str(), task.steps[0].seq,devId, chnNo);

	// 填充保护条件
	tp.dest = (task.boostTemp << 16) | (chnNo << 8) | devId;
	tp.boostVoltage = task.boostVoltage;
	tp.tempRange = (task.maxTemp << 16) | (task.minTemp);
	tp.maxVoltage = task.maxVoltage;
	tp.minVoltage = task.minVoltage;
	tp.maxCurrent = task.maxCurrent;
	tp.minCurrent = task.minCurrent;

	// 检查设备状态
	ret = gDataManager.getDevState(devId);
	if(ret != IE_OK)
	{
		printf("dev offline  ret code %d!\n",ret);
		return ret;	// 设备离线返回
	}

	ret = -1;
	while(ret != IE_OK)  // 循环下发保护条件直到成功
	{
		can0.setNotify(tp);
		printf(">>>...task protect notify!\n");
		ret = can0.setWaitAck(&can_id, 5); // 5秒超时后重新发送
		i++;
		if(i == 3)
		{
			return ret;	// 超时返回
		}
	}


	TaskSchedule *pTaskSchedule = new TaskSchedule(task);
	pTaskSchedule->start();
	printf("thread for task[%s]  @ taskTab[%x][%x]!\n",pTaskSchedule->name().c_str(),task.devId,task.chnNo);
	_taskTab[task.devId-1][task.chnNo-1] = pTaskSchedule;

	return IE_OK;	// 正常返回
}

s32
TaskManager::startTask(u8 devId, u8 chnNo)
{
	return 0;
}

s32
TaskManager::pauseTask(u8 devId, u8 chnNo)
{
	TaskSchedule *pTask;
	pTask = _taskTab[devId-1][chnNo-1];
	if(pTask != NULL)
	{
		pTask->pause();
		return IE_OK;
	}
	else
	{
		return IE_TASKCTRL_NULLPTR;
	}
}

s32
TaskManager::contTask(u8 devId, u8 chnNo)
{
	TaskSchedule *pTask;
	pTask = _taskTab[devId-1][chnNo-1];
	if(pTask != NULL)
	{
		pTask->cont();
		return IE_OK;
	}
	else
	{
		return IE_TASKCTRL_NULLPTR;
	}
}

s32
TaskManager::stopTask(u8 devId, u8 chnNo)
{
	TaskSchedule *pTask;
	pTask = _taskTab[devId-1][chnNo-1];
	if(pTask != NULL)
	{
		pTask->stop();
		return IE_OK;
	}
	else
	{
		return IE_TASKCTRL_NULLPTR;
	}
}

s32
TaskManager::clearTask(u8 devId, u8 chnNo)
{
	_taskTab[devId][chnNo] = NULL; // clear task handler
	//TODO reset chn state
	gDataManager.chnReset(devId,chnNo);

	// IDLE CMD SEND

	printf("task [%x][%d] cleared !\n",devId,chnNo);
}

// 通知指定任务当前工步结束
s32
TaskManager::notifyEnd(u8 devId, u8 chnNo)
{
	TaskSchedule *pTask;
	struct can_frame fakeFrame;
	fakeFrame.can_id = ((chnNo << 8) & PKT_CHN_MASK) | devId;
	memset(fakeFrame.data, 0, 8);
	pTask = _taskTab[devId-1][chnNo-1];
	if(pTask != NULL)
	{
		pTask->putAck(fakeFrame);
		return IE_OK;
	}
	else
	{
		return IE_TASKCTRL_NULLPTR;
	}
}

s32
TaskManager::processAck(struct can_frame frame)
{
	TaskSchedule *pTask;
	u8 devId, chnNo;

	devId = (u8)(frame.can_id & PKT_ID_MASK);
	chnNo = (u8)((frame.can_id & PKT_CHN_MASK) >> 8);
	pTask = _taskTab[devId-1][chnNo-1];
	if(pTask != NULL)
	{
		pTask->putAck(frame);
		return IE_OK;
	}
	else
	{
		return IE_TASKCTRL_NULLPTR;
	}
}

u32
TaskManager::getTaskEla(u8 devId, u8 chnNo)
{
	TaskSchedule *pTask;
	u32 ms = 0;

	pTask = _taskTab[devId-1][chnNo-1];
	if(pTask != NULL)
	{
		ms = pTask->getTaskElapsed();
	}
	return ms;
}


u32
TaskManager::getStepEla(u8 devId, u8 chnNo)
{
	TaskSchedule *pTask;
	u32 ms = 0;

	pTask = _taskTab[devId-1][chnNo-1];
	if(pTask != NULL)
	{
		ms = pTask->getStepElapsed();
	}
	return ms;
}
